#include "offsets.h"

FLOAT_XYZ   acc_sample[6],acc_add,Acc_off,Acc_scale,acc_off,acc_scale;
FLOAT_XYZ		gyr_add,gyr_off,Gyr_off;
float Gyr_Buffer[3],Acc_Buffer[6],Gyr_Off[3],Acc_Off[6];;
int16 Mag_off[3];
int imu_off_flag=0;

/*********************************************************************************************************
*函  数：void IMU_OFFSETS(void)
*功　能：陀螺仪和加速度计校准，并将校准后的数据存入AT24C02芯片中
*参  数：无
*返回值：无
*备  注：对陀螺仪使用均值校准，加速度计使用六面校准法校准

注：进行校正前，需将任务定时器中的任务注释掉。

校准教程：
1、遥控器倒八字且SWC打在中间，此时开始IMU校准
2、无人机正常放置，此时将SWA第一次打到下，指示灯变紫红，五秒内将SWA打到上，待指示灯变蓝，表示陀螺仪校准完成
3、无人机依此呈Z正面朝上、X正面朝上，X正面朝下，Y正面朝下，Y正面朝上，Z正面朝下放置，此时将SWA第二到七次打到下，操作同2，此时进行加速度计六面校准
4、完成以上操作后，SWA第八次打下，此时将数据进行存储，绿灯亮完成结束校准
**********************************************************************************************************/	
void Imu_Offset(void)
{
	static int i=0,gyr_flag=1,acc_flag=1;
	acc_scale.X=1;//零偏清零
	acc_scale.Y=1;
	acc_scale.Z=1;
	acc_off.X=0;
	acc_off.Y=0;
	acc_off.Z=0;
	gyr_off.X=0;
	gyr_off.Y=0;
	gyr_off.Z=0;
	
	while(imu_off_flag<9)
	{
		if(PPM_Databuf[4]>1800&&PPM_Databuf[4]<2200) imu_off_flag++; 
		
			if(imu_off_flag==1&&gyr_flag)
			{
				RGB_LED_Violet();//紫红灯
				
				while(i<10000)
				{
						i++;
						IMU();
						gyr_add.X+=Gyr_rad.X;
						gyr_add.Y+=Gyr_rad.Y;
						gyr_add.Z+=Gyr_rad.Z;

				}
					Gyr_off.X=gyr_add.X/10000.0f;
					Gyr_off.Y=gyr_add.Y/10000.0f;
					Gyr_off.Z=gyr_add.Z/10000.0f;
					RGB_LED_Blue();//蓝灯
					i=0;
					gyr_flag=0;
				}
		if(imu_off_flag>=2&&imu_off_flag<=7&&imu_off_flag>acc_flag)
		{
			acc_flag=imu_off_flag;
			RGB_LED_Violet();//紫红灯
			
				while(i<2000)
				{
					i++;
					IMU();
					acc_add.X+=Acc_rad.X;
					acc_add.Y+=Acc_rad.Y; 
					acc_add.Z+=Acc_rad.Z;
				}
				acc_sample[imu_off_flag-2].X=acc_add.X/2000.0f;
				acc_sample[imu_off_flag-2].Y=acc_add.Y/2000.0f;
				acc_sample[imu_off_flag-2].Z=acc_add.Z/2000.0f;
				acc_add.X=acc_add.Y=acc_add.Z=0;
				i=0;
				RGB_LED_Blue();//蓝灯
		}
		if(imu_off_flag==8)
			{
				Calibrate_accel(acc_sample,&Acc_off,&Acc_scale);
				Gyr_Buffer[0]=Gyr_off.X;
				Gyr_Buffer[1]=Gyr_off.Y;
				Gyr_Buffer[2]=Gyr_off.Z;
				Acc_Buffer[0]=Acc_off.X;
				Acc_Buffer[1]=Acc_off.Y;
				Acc_Buffer[2]=Acc_off.Z;
				Acc_Buffer[3]=Acc_scale.X;
				Acc_Buffer[4]=Acc_scale.Y;
				Acc_Buffer[5]=Acc_scale.Z;
				write_AT24C02(0,Gyr_Buffer,12);
				delay_ms(10);
				write_AT24C02(20,Acc_Buffer,24);
				RGB_LED_Green();//绿灯
			}
	}
}



/*********************************************************************************************************
磁力计校准操作指南:
1、遥控器倒八字且SWC位于最下开始校准，此时指示灯变紫红
2、将SWA打下，飞机任意全向缓速旋转，旋转完后将SWA打上
3、最后将SWC打到最上，完成校准，此时指示灯变蓝

**********************************************************************************************************/	
int16_t x_max ;
int16_t x_min ;
int16_t y_max ;
int16_t y_min ;
int16_t z_max ;
int16_t z_min ;
int16_t Mag_Max ;
INT16_XYZ Mag_data;
int16_t MAG_OFF[3];
float MAG_SCALE[3];
int Mag_flag=1;
void Mag_Offset(void)
{
	static int flag=0;
	RGB_LED_Violet();//紫红灯

	Mag_Off.X=0;//零偏清零
	Mag_Off.Y=0;
	Mag_Off.Z=0;
	
	
	while(Mag_flag==1)
	{
		Mag_data=QMC58831L_data;  //选择需要校准的磁力计
		if(PPM_Databuf[4]>800&&PPM_Databuf[4]<1200) flag=0;//SWA打上
	  if(PPM_Databuf[4]>1800&&PPM_Databuf[4]<2200) flag=1;//SWA打下
		if(flag==1) 
		{
			if(Mag_data.X >= x_max)   x_max = (int16_t)(Mag_data.X);  
			if(Mag_data.X <  x_min)   x_min = (int16_t)(Mag_data.X);
			if(Mag_data.Y >= y_max)   y_max = (int16_t)(Mag_data.Y);  
			if(Mag_data.Y <  y_min)   y_min = (int16_t)(Mag_data.Y);
			if(Mag_data.Z >= z_max)   z_max = (int16_t)(Mag_data.Z);
			if(Mag_data.Z <  z_min)   z_min = (int16_t)(Mag_data.Z);
		}
		if(PPM_Databuf[6]>800&&PPM_Databuf[6]<1200) 
		{
			
			MAG_OFF[0]=(x_min+x_max)/2.0;
			MAG_OFF[1]=(y_min+y_max)/2.0;
			MAG_OFF[2]=(z_min+z_max)/2.0;	
			
			write_AT24C02(13,MAG_OFF,6);
			
			Mag_flag=0;
			RGB_LED_Green();//绿灯
		}
	}
}

/*********************************************************************************************************
*函  数：void IMU_OFF_Read(void)
*功　能：读校准后AT24C02芯片中的校准数据
*参  数：无
*返回值：无
*备  注：无
**********************************************************************************************************/	
void Offset_Read(void)
{
	gyr_off.X=0.00347822143f;
	gyr_off.Y=-0.00408279548f;
	gyr_off.Z=-0.00005108697f;
	acc_off.X=0.0703816f;
	acc_off.Y=-0.104485f;
	acc_off.Z=0.0296948f;
	acc_scale.X=1.00769f;
	acc_scale.Y=1.00713f;
	acc_scale.Z=1.00462f;
//	Mag_Off.X=-1052; 
//	Mag_Off.Y=-591; 
//	Mag_Off.Z=-197; 
}

